/*
 * Agent.h
 * RVO2 Library
 *
 * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill
 * SPDX-License-Identifier: Apache-2.0
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Please send all bug reports to <geom@cs.unc.edu>.
 *
 * The authors may be contacted via:
 *
 * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
 * Dept. of Computer Science
 * 201 S. Columbia St.
 * Frederick P. Brooks, Jr. Computer Science Bldg.
 * Chapel Hill, N.C. 27599-3175
 * United States of America
 *
 * <https://gamma.cs.unc.edu/RVO2/>
 */

import { KdTree } from "./KdTree";
import { Line } from "./Line";
import { Obstacle } from "./Obstacle";
import { RvoPool, IRvoPoolItem } from "./RvoPool";
import { RVO_EPSILON, Vector2 } from "./Vector2";
import { IRVOArrayItem, RVOArray } from "./RVOArrayEx";

const _arr1: RVOArray<Line> = new RVOArray();
const _arr2: RVOArray<Line> = new RVOArray();

// for linearProgram1
const _v1: Vector2 = new Vector2;

// for linearProgram2
const _v2: Vector2 = new Vector2;
const _v3: Vector2 = new Vector2;

// for linearProgram3
const _v4: Vector2 = new Vector2;
const _v5: Vector2 = new Vector2;

// for computeNewVelocity
const _v6: Vector2 = new Vector2;
const _v7: Vector2 = new Vector2;
const _v8: Vector2 = new Vector2;
const _v9: Vector2 = new Vector2;
const _v10: Vector2 = new Vector2;
const _v11: Vector2 = new Vector2;
const _v12: Vector2 = new Vector2;
const _v13: Vector2 = new Vector2;
const _v14: Vector2 = new Vector2;

//for insertObstacleNeighbor
const _v15: Vector2 = new Vector2;
const _v16: Vector2 = new Vector2;
const _v17: Vector2 = new Vector2;

//for other
const _v18: Vector2 = new Vector2;

/**
 * @relates        Agent
 * @brief          Solves a one-dimensional linear program on a specified line
 *                 subject to linear constraints defined by lines and a circular
 *                 constraint.
 * @param[in]      lines        Lines defining the linear constraints.
 * @param[in]      lineNo       The specified line constraint.
 * @param[in]      radius       The radius of the circular constraint.
 * @param[in]      optVelocity  The optimization velocity.
 * @param[in]      directionOpt True if the direction should be optimized.
 * @param[in, out] result       A reference to the result of the linear program.
 * @return         True if successful.
 */
function linearProgram1(lines: RVOArray<Line>, lineNo: number,
  radius: number, optVelocity: Vector2, directionOpt: boolean, result: Vector2) {
  /* NOLINT(runtime/references) */
  let dotProduct = lines.get(lineNo).point.dot(lines.get(lineNo).direction);
  let discriminant =
    dotProduct * dotProduct + radius * radius - Vector2.absSq(lines.get(lineNo).point);

  if (discriminant < 0) {
    /* Max speed circle fully invalidates line lineNo. */
    return false;
  }

  let sqrtDiscriminant = Math.sqrt(discriminant);
  let tLeft = -dotProduct - sqrtDiscriminant;
  let tRight = -dotProduct + sqrtDiscriminant;

  for (let i = 0; i < lineNo; ++i) {
    let denominator = Vector2.det(lines.get(lineNo).direction, lines.get(i).direction);
    let numerator = Vector2.det(lines.get(i).direction, lines.get(lineNo).point.sub(lines.get(i).point, _v1));

    if (Math.abs(denominator) <= RVO_EPSILON) {
      /* Lines lineNo and i are (almost) parallel. */
      if (numerator < 0) {
        return false;
      }

      continue;
    }

    let t = numerator / denominator;

    if (denominator >= 0) {
      /* Line i bounds line lineNo on the right. */
      tRight = Math.min(tRight, t);
    } else {
      /* Line i bounds line lineNo on the left. */
      tLeft = Math.max(tLeft, t);
    }

    if (tLeft > tRight) {
      return false;
    }
  }

  if (directionOpt) {
    /* Optimize direction. */
    if (optVelocity.dot(lines.get(lineNo).direction) > 0) {
      /* Take right extreme. */
      lines.get(lineNo).direction.mulScalar(tRight, _v1).add(lines.get(lineNo).point);
    } else {
      /* Take left extreme. */
      lines.get(lineNo).direction.mulScalar(tLeft, _v1).add(lines.get(lineNo).point);
    }
    result.set(_v1);
  } else {
    /* Optimize closest point. */
    let t = lines.get(lineNo).direction.dot(optVelocity.sub(lines.get(lineNo).point, _v1));

    if (t < tLeft) {
      lines.get(lineNo).direction.mulScalar(tLeft, _v1).add(lines.get(lineNo).point);
    } else if (t > tRight) {
      lines.get(lineNo).direction.mulScalar(tRight, _v1).add(lines.get(lineNo).point);
    } else {
      lines.get(lineNo).direction.mulScalar(t, _v1).add(lines.get(lineNo).point);
    }
    result.set(_v1);
  }

  return true;
}

/**
* @relates        Agent
* @brief          Solves a two-dimensional linear program subject to linear
*                 constraints defined by lines and a circular constraint.
* @param[in]      lines        Lines defining the linear constraints.
* @param[in]      radius       The radius of the circular constraint.
* @param[in]      optVelocity  The optimization velocity.
* @param[in]      directionOpt True if the direction should be optimized.
* @param[in, out] result       A reference to the result of the linear program.
* @return         The number of the line it fails on, and the number of lines
*                 if successful.
*/
function linearProgram2(lines: RVOArray<Line>, radius: number,
  optVelocity: Vector2, directionOpt: boolean, result: Vector2) {
  /* NOLINT(runtime/references) */
  if (directionOpt) {
    /* Optimize direction. Note that the optimization velocity is of unit length
    * in this case.
    */
    result.set(optVelocity.mulScalar(radius, _v2));
  } else if (Vector2.absSq(optVelocity) > radius * radius) {
    /* Optimize closest point and outside circle. */
    result.set(optVelocity.normalize(_v2).mulScalar(radius));
  } else {
    /* Optimize closest point and inside circle. */
    result.set(optVelocity);
  }

  let temp = _v3;

  for (let i = 0; i < lines.length; ++i) {
    if (Vector2.det(lines.get(i).direction, lines.get(i).point.sub(result, _v2)) > 0) {
      /* Result does not satisfy constraint i. Compute new optimal result. */
      temp.set(result);
      if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
        result.set(temp);
        return i;
      }
    }
  }

  return lines.length;
}

/**
* @relates        Agent
* @brief          Solves a two-dimensional linear program subject to linear
*                 constraints defined by lines and a circular constraint.
* @param[in]      lines        Lines defining the linear constraints.
* @param[in]      numObstLines Count of obstacle lines.
* @param[in]      beginLine    The line on which the 2-d linear program failed.
* @param[in]      radius       The radius of the circular constraint.
* @param[in, out] result       A reference to the result of the linear program.
*/
function linearProgram3(lines: RVOArray<Line>, numObstLines: number,
  beginLine: number, radius: number, result: Vector2) {
  /* NOLINT(runtime/references) */
  let distance = 0;
  let temp = _v4;
  for (let i = beginLine; i < lines.length; ++i) {
    if (Vector2.det(lines.get(i).direction, lines.get(i).point.sub(result, _v5)) > distance) {
      /* Result does not satisfy constraint of line i. */

      let projLines: RVOArray<Line> = lines.slice(0, numObstLines, _arr1);

      for (let j = numObstLines; j < i; ++j) {
        let line: Line = Line.pool.get();

        let determinant = Vector2.det(lines.get(i).direction, lines.get(j).direction);

        if (Math.abs(determinant) <= RVO_EPSILON) {
          /* Line i and line j are parallel. */
          if (lines.get(i).direction.dot(lines.get(j).direction) > 0) {
            /* Line i and line j point in the same direction. */
            continue;
          }

          /* Line i and line j point in opposite direction. */
          line.point.set(lines.get(i).point.add(lines.get(j).point, _v5).mulScalar(0.5));
        } else {
          lines.get(i).direction.mulScalar(Vector2.det(lines.get(j).direction, lines.get(i).point.sub(lines.get(j).point, _v5)) / determinant,
            _v5).add(lines.get(i).point);
          line.point.set(_v5);
        }
        lines.get(j).direction.sub(lines.get(i).direction, _v5).normalize();
        line.direction.set(_v5);
        projLines.push(line);
        _arr2.push(line);
      }

      temp.set(result);

      if (linearProgram2(
        projLines, radius,
        _v5.set(-lines.get(i).direction.y_, lines.get(i).direction.x_), true,
        result) < projLines.length) {
        /* This should in principle not happen. The result is by definition
        * already in the feasible region of this linear program. If it fails,
        * it is due to small floating point error, and the current result is
        * kept. */
        result.set(temp);
      }

      projLines.length = 0;
      RvoPool.recycleFromArray(_arr2, Line.pool);

      distance = Vector2.det(lines.get(i).direction, lines.get(i).point.sub(result, _v5));
    }
  }
}
/* namespace */

class AgentNeighborInfo implements IRVOArrayItem, IRvoPoolItem {
  static pool: RvoPool<AgentNeighborInfo> = new RvoPool(AgentNeighborInfo, 256);

  inPool: boolean;
  rvoIndex: number;
  dist: number;
  agent: Agent;

  reuse(): void {

  }

  unuse(): void {
    this.agent = null;
  }
}

class ObstacleNeighborInfo implements IRVOArrayItem, IRvoPoolItem {
  static pool: RvoPool<ObstacleNeighborInfo> = new RvoPool(ObstacleNeighborInfo, 256);

  inPool: boolean;
  rvoIndex: number;
  dist: number;
  obstacle: Obstacle;

  reuse(): void {

  }

  unuse(): void {
    this.obstacle = null;
  }
}

/**
 * @brief Defines an agent in the simulation.
 */
export class Agent implements IRVOArrayItem, IRvoPoolItem {
  static pool: RvoPool<Agent> = new RvoPool(Agent, 256, 1024);

  private _rvoIndex: number;
  public get rvoIndex(): number {
    return this._rvoIndex;
  }
  public set rvoIndex(value: number) {
    this._rvoIndex = value;
  }
  
  inPool: boolean;

  public constructor() {
    this.maxNeighbors_ = 0;
    this.maxSpeed_ = 0;
    this.neighborDist_ = 0;
    this.radius_ = 0;
    this.timeHorizon_ = 0;
    this.timeHorizonObst_ = 0;

    this.agentNeighbors_ = new RVOArray();
    this.obstacleNeighbors_ = new RVOArray();
    this.orcaLines_ = new RVOArray();

    this.newVelocity_ = Vector2.pool.get();
    this.position_ = Vector2.pool.get();
    this.prefVelocity_ = Vector2.pool.get();
    this.velocity_ = Vector2.pool.get();
    this.enable = true;
  }

  reuse(): void {
    this.enable = true;
  }

  unuse(): void {
    this.syncCallback = null;
    this.syncTarget = null;
    this.prefVelocity_.set(0, 0);
    this.newVelocity_.set(0, 0);
  }

  /**
   * @brief     Computes the neighbors of this agent.
   * @param[in] kdTree A pointer to the k-D trees for agents and static
   *                   obstacles in the simulation.
   */
  public computeNeighbors(kdTree: KdTree) {
    if (!this.enable) {
      // this.obstacleNeighbors_.length = 0;
      // this.agentNeighbors_.length = 0;
      RvoPool.recycleFromArray(this.obstacleNeighbors_, ObstacleNeighborInfo.pool);
      RvoPool.recycleFromArray(this.agentNeighbors_, AgentNeighborInfo.pool);
      return;
    }
    // this.obstacleNeighbors_.length = 0;
    RvoPool.recycleFromArray(this.obstacleNeighbors_, ObstacleNeighborInfo.pool);
    let range = this.timeHorizonObst_ * this.maxSpeed_ + this.radius_;
    kdTree.computeObstacleNeighbors(this, range * range);

    // this.agentNeighbors_.length = 0;
    RvoPool.recycleFromArray(this.agentNeighbors_, AgentNeighborInfo.pool);

    if (this.maxNeighbors_ > 0) {
      let rangeSq = this.neighborDist_ * this.neighborDist_;
      kdTree.computeAgentNeighbors(this, rangeSq);
    }
  }

  /**
   * @brief     Computes the new velocity of this agent.
   * @param[in] timeStep The time step of the simulation.
   */
  public computeNewVelocity(timeStep: number) {
    if (!this.enable) {
      this.newVelocity_.set(this.prefVelocity_);
      return;
    }

    // recycle(this.orcaLines_, Line.pool);
    this.orcaLines_.length = 0;
    let invTimeHorizonObst = 1 / this.timeHorizonObst_;
    /* Create obstacle ORCA lines. */
    for (let i = 0; i < this.obstacleNeighbors_.length; ++i) {
      let obstacle1 = this.obstacleNeighbors_.get(i).obstacle;
      let obstacle2 = obstacle1.next_;

      let relativePosition1 = obstacle1.point_.sub(this.position_, _v6);
      let relativePosition2 = obstacle2.point_.sub(this.position_, _v7);

      /* Check if velocity obstacle of obstacle is already taken care of by
       * previously constructed obstacle ORCA lines. */
      let alreadyCovered = false;


      for (let j = 0; j < this.orcaLines_.length; ++j) {
        if (Vector2.det(
          relativePosition1.mulScalar(invTimeHorizonObst, _v13).sub(this.orcaLines_.get(j).point, _v13),
          this.orcaLines_.get(j).direction) -
          invTimeHorizonObst * this.radius_ >=
          -RVO_EPSILON &&
          Vector2.det(
            relativePosition2.mulScalar(invTimeHorizonObst, _v13).sub(this.orcaLines_.get(j).point, _v13),
            this.orcaLines_.get(j).direction) -
          invTimeHorizonObst * this.radius_ >=
          -RVO_EPSILON) {
          alreadyCovered = true;
          break;
        }
      }

      if (alreadyCovered) {
        continue;
      }

      /* Not yet covered. Check for collisions. */
      let distSq1 = Vector2.absSq(relativePosition1);
      let distSq2 = Vector2.absSq(relativePosition2);

      let radiusSq = this.radius_ * this.radius_;

      let obstacleVector = obstacle2.point_.sub(obstacle1.point_, _v13);
      let s = -relativePosition1.dot(obstacleVector) / Vector2.absSq(obstacleVector);

      let distSqLine = Vector2.absSq(obstacleVector.mulScalar(-s, _v14).sub(relativePosition1));

      let line = Line.pool.get();

      if (s < 0 && distSq1 <= radiusSq) {
        /* Collision with left vertex. Ignore if non-convex. */
        if (obstacle1.isConvex_) {
          line.point.set(0, 0);
          line.direction.set(-relativePosition1.y_, relativePosition1.x_).normalize();
          this.orcaLines_.push(line);
        } else {
          Line.pool.put(line);
        }

        continue;
      }

      if (s > 1 && distSq2 <= radiusSq) {
        /* Collision with right vertex. Ignore if non-convex or if it will be
         * taken care of by neighoring obstace */
        if (obstacle2.isConvex_ &&
          Vector2.det(relativePosition2, obstacle2.direction_) >= 0) {
          line.point.set(0, 0);
          line.direction.set(-relativePosition2.y_, relativePosition2.x_).normalize();
          this.orcaLines_.push(line);
        } else {
          Line.pool.put(line);
        }

        continue;
      }

      if (s >= 0 && s <= 1 && distSqLine <= radiusSq) {
        /* Collision with obstacle segment. */
        line.point.set(0, 0);
        line.direction.set(obstacle1.direction_).mulScalar(-1);
        this.orcaLines_.push(line);
        continue;
      }

      /* No collision. Compute legs. When obliquely viewed, both legs can come
       * from a single vertex. Legs extend cut-off line when nonconvex vertex. */
      let leftLegDirection: Vector2 = _v8;
      let rightLegDirection: Vector2 = _v9;

      if (s < 0 && distSqLine <= radiusSq) {
        /* Obstacle viewed obliquely so that left vertex defines velocity
         * obstacle. */
        if (!obstacle1.isConvex_) {
          /* Ignore obstacle. */
          Line.pool.put(line);
          continue;
        }

        obstacle2 = obstacle1;

        let leg1 = Math.sqrt(distSq1 - radiusSq);
        leftLegDirection.set(
          relativePosition1.x_ * leg1 - relativePosition1.y_ * this.radius_,
          relativePosition1.x_ * this.radius_ + relativePosition1.y_ * leg1
        ).divScalar(distSq1);
        rightLegDirection.set(
          relativePosition1.x_ * leg1 + relativePosition1.y_ * this.radius_,
          -relativePosition1.x_ * this.radius_ + relativePosition1.y_ * leg1
        ).divScalar(distSq1);
      } else if (s > 1 && distSqLine <= radiusSq) {
        /* Obstacle viewed obliquely so that right vertex defines velocity
         * obstacle. */
        if (!obstacle2.isConvex_) {
          /* Ignore obstacle. */
          Line.pool.put(line);
          continue;
        }

        obstacle1 = obstacle2;

        let leg2 = Math.sqrt(distSq2 - radiusSq);
        leftLegDirection.set(
          relativePosition2.x_ * leg2 - relativePosition2.y_ * this.radius_,
          relativePosition2.x_ * this.radius_ + relativePosition2.y_ * leg2
        ).divScalar(distSq2);
        rightLegDirection.set(
          relativePosition2.x_ * leg2 + relativePosition2.y_ * this.radius_,
          -relativePosition2.x_ * this.radius_ + relativePosition2.y_ * leg2
        ).divScalar(distSq2);
      } else {
        /* Usual situation. */
        if (obstacle1.isConvex_) {
          let leg1 = Math.sqrt(distSq1 - radiusSq);
          leftLegDirection.set(relativePosition1.x_ * leg1 - relativePosition1.y_ * this.radius_,
            relativePosition1.x_ * this.radius_ + relativePosition1.y_ * leg1
          ).divScalar(distSq1);
        } else {
          /* Left vertex non-convex; left leg extends cut-off line. */
          leftLegDirection.set(obstacle1.direction_).mulScalar(-1);
        }

        if (obstacle2.isConvex_) {
          let leg2 = Math.sqrt(distSq2 - radiusSq);
          rightLegDirection.set(
            relativePosition2.x_ * leg2 + relativePosition2.y_ * this.radius_,
            -relativePosition2.x_ * this.radius_ + relativePosition2.y_ * leg2
          ).divScalar(distSq2);
        } else {
          /* Right vertex non-convex; right leg extends cut-off line. */
          rightLegDirection.set(obstacle1.direction_);
        }
      }

      /* Legs can never point into neighboring edge when convex vertex, take
       * cutoff-line of neighboring edge instead. If velocity projected on
       * "foreign" leg, no constraint is added. */
      let leftNeighbor = obstacle1.previous_;

      let isLeftLegForeign = false;
      let isRightLegForeign = false;

      if (obstacle1.isConvex_ &&
        Vector2.det(leftLegDirection, leftNeighbor.direction_.mulScalar(-1, _v13)) >= 0) {
        /* Left leg points into obstacle. */
        leftLegDirection.set(leftNeighbor.direction_).mulScalar(-1);
        isLeftLegForeign = true;
      }

      if (obstacle2.isConvex_ &&
        Vector2.det(rightLegDirection, obstacle2.direction_) <= 0) {
        /* Right leg points into obstacle. */
        rightLegDirection.set(obstacle2.direction_);
        isRightLegForeign = true;
      }

      /* Compute cut-off centers. */
      let leftCutoff = _v10.set(obstacle1.point_).sub(this.position_).mulScalar(invTimeHorizonObst);
      let rightCutoff = _v11.set(obstacle2.point_).sub(this.position_).mulScalar(invTimeHorizonObst);
      let cutoffVector = _v12.set(rightCutoff).sub(leftCutoff);

      /* Project current velocity on velocity obstacle. */

      /* Check if current velocity is projected on cutoff circles. */
      let t =
        obstacle1 == obstacle2
          ? 0.5
          : this.velocity_.sub(leftCutoff, _v13).dot(cutoffVector) / Vector2.absSq(cutoffVector);
      let tLeft = this.velocity_.sub(leftCutoff, _v13).dot(leftLegDirection);
      let tRight = this.velocity_.sub(rightCutoff, _v13).dot(rightLegDirection);

      if ((t < 0 && tLeft < 0) ||
        (obstacle1 == obstacle2 && tLeft < 0 && tRight < 0)) {
        /* Project on left cut-off circle. */
        let unitW = this.velocity_.sub(leftCutoff, _v13).normalize();
        line.direction.set(unitW.y_, -unitW.x_);
        line.point.set(unitW).mulScalar(this.radius_ * invTimeHorizonObst).add(leftCutoff);
        this.orcaLines_.push(line);
        continue;
      }

      if (t > 1 && tRight < 0) {
        /* Project on right cut-off circle. */
        let unitW = this.velocity_.sub(rightCutoff, _v13).normalize();
        line.direction.set(unitW.y_, -unitW.x_);
        line.point.set(unitW).mulScalar(this.radius_ * invTimeHorizonObst).add(rightCutoff);
        this.orcaLines_.push(line);
        continue;
      }

      /* Project on left leg, right leg, or cut-off line, whichever is closest to
       * velocity. */
      let distSqCutoff =
        (t < 0 || t > 1 || obstacle1 == obstacle2)
          ? Infinity
          : cutoffVector.mulScalar(t, _v13).add(leftCutoff)
            .mulScalar(-1).add(this.velocity_).absSq();
      let distSqLeft =
        tLeft < 0
          ? Infinity
          : leftLegDirection.mulScalar(tLeft, _v13).add(leftCutoff)
            .mulScalar(-1).add(this.velocity_).absSq();
      let distSqRight =
        tRight < 0
          ? Infinity
          : rightLegDirection.mulScalar(tRight, _v13).add(rightCutoff)
            .mulScalar(-1).add(this.velocity_).absSq();

      if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
        /* Project on cut-off line. */
        line.direction.set(obstacle1.direction_).mulScalar(-1);

        line.point.set(-line.direction.y_, line.direction.x_).mulScalar(this.radius_ * invTimeHorizonObst).add(leftCutoff);
        this.orcaLines_.push(line);
        continue;
      }

      if (distSqLeft <= distSqRight) {
        /* Project on left leg. */
        if (isLeftLegForeign) {
          Line.pool.put(line);
          continue;
        }
        line.direction.set(leftLegDirection);
        line.point.set(-line.direction.y_, line.direction.x_).mulScalar(this.radius_ * invTimeHorizonObst).add(leftCutoff);
        this.orcaLines_.push(line);
        continue;
      }

      /* Project on right leg. */
      if (isRightLegForeign) {
        Line.pool.put(line);
        continue;
      }
      line.direction.set(-rightLegDirection.x_, -rightLegDirection.y_);
      line.point.set(-line.direction.y_, line.direction.x_).mulScalar(this.radius_ * invTimeHorizonObst).add(rightCutoff);
      this.orcaLines_.push(line);
    }

    let numObstLines = this.orcaLines_.length;

    let invTimeHorizon = 1 / this.timeHorizon_;


    /* Create agent ORCA lines. */

    for (let i = 0; i < this.agentNeighbors_.length; ++i) {
      let other = this.agentNeighbors_.get(i).agent;
      let relativePosition = other.position_.sub(this.position_, _v6);
      let relativeVelocity = this.velocity_.sub(other.velocity_, _v7);

      //FIX: 当相对位置和速度都为零时，后续除法计算为NaN，导致避障失效
      if (relativePosition.x_ === 0 && relativePosition.y_ === 0 &&
        relativeVelocity.x_ === 0 && relativeVelocity.y_ === 0) {
        continue;
      }

      let distSq = relativePosition.absSq();
      let combinedRadius = this.radius_ + other.radius_;
      let combinedRadiusSq = combinedRadius * combinedRadius;

      let line = Line.pool.get();
      let u: Vector2;

      if (distSq > combinedRadiusSq) {
        /* No collision. */
        let w = relativePosition.mulScalar(invTimeHorizon, _v10).mulScalar(-1).add(relativeVelocity);
        // relativeVelocity - invTimeHorizon * relativePosition;
        /* Vector from cutoff center to relative velocity. */
        let wLengthSq = w.absSq();

        let dotProduct = w.dot(relativePosition);

        if (dotProduct < 0 &&
          dotProduct * dotProduct > combinedRadiusSq * wLengthSq) {
          /* Project on cut-off circle. */
          let wLength = Math.sqrt(wLengthSq);
          let unitW = w.divScalar(wLength, _v8);

          line.direction.set(unitW.y_, -unitW.x_);
          u = unitW.mulScalar(combinedRadius * invTimeHorizon - wLength, _v9);
        } else {
          /* Project on legs. */
          let leg = Math.sqrt(distSq - combinedRadiusSq);

          if (Vector2.det(relativePosition, w) > 0) {
            /* Project on left leg. */
            line.direction.set(relativePosition.x_ * leg -
              relativePosition.y_ * combinedRadius,
              relativePosition.x_ * combinedRadius +
              relativePosition.y_ * leg).divScalar(distSq);
          } else {
            /* Project on right leg. */
            line.direction.set(relativePosition.x_ * leg -
              relativePosition.y_ * combinedRadius,
              -relativePosition.x_ * combinedRadius +
              relativePosition.y_ * leg).divScalar(distSq);
          }
          u = line.direction.mulScalar(line.direction.dot(relativeVelocity), _v9).sub(relativeVelocity);
        }
      } else {
        /* Collision. Project on cut-off circle of time timeStep. */
        let invTimeStep = 1 / timeStep;

        /* Vector from cutoff center to relative velocity. */
        let w = relativePosition.mulScalar(invTimeStep, _v8).mulScalar(-1).add(relativeVelocity);

        let wLength = w.abs();
        let unitW = w.divScalar(wLength, _v11);

        line.direction.set(unitW.y_, -unitW.x_);
        u = unitW.mulScalar(combinedRadius * invTimeStep - wLength, _v9);
      }

      line.point.set(u).mulScalar(0.5).add(this.velocity_);
      this.orcaLines_.push(line);
    }


    let lineFail = linearProgram2(this.orcaLines_, this.maxSpeed_, this.prefVelocity_, false, this.newVelocity_);


    if (lineFail < this.orcaLines_.length) {
      linearProgram3(this.orcaLines_, numObstLines, lineFail, this.maxSpeed_, this.newVelocity_);
    }

    RvoPool.recycleFromArray(this.orcaLines_, Line.pool);
  }

  /**
   * @brief          Inserts an agent neighbor into the set of neighbors of this
   *                 agent.
   * @param[in]      agent   A pointer to the agent to be inserted.
   * @param[in, out] rangeSq The squared range around this agent.
   */
  public insertAgentNeighbor(agent: Agent, rangeSq: number) {
    if (this != agent && this.enable) {
      let distSq = Vector2.absSq(this.position_.sub(agent.position_, _v18));

      if (distSq < rangeSq) {
        if (this.agentNeighbors_.length < this.maxNeighbors_) {
          let info = AgentNeighborInfo.pool.get();
          info.dist = distSq;
          info.agent = agent;
          this.agentNeighbors_.push(info);
        }

        let i = this.agentNeighbors_.length - 1;
        while (i != 0 && distSq < this.agentNeighbors_.get(i - 1).dist) {
          // this.agentNeighbors_.set(i, this.agentNeighbors_.get(i - 1));
          this.agentNeighbors_.get(i).dist = this.agentNeighbors_.get(i - 1).dist;
          this.agentNeighbors_.get(i).agent = this.agentNeighbors_.get(i - 1).agent;
          --i;
        }

        let info = this.agentNeighbors_.get(i);
        info.dist = distSq;
        info.agent = agent;
        // this.agentNeighbors_.set(i, info);

        if (this.agentNeighbors_.length == this.maxNeighbors_) {
          rangeSq = this.agentNeighbors_.get(this.maxNeighbors_ - 1).dist;
        }
      }
    }
  } /* NOLINT(runtime/references) */

  /**
   * @brief          Inserts a static obstacle neighbor into the set of
   *                 neighbors of this agent.
   * @param[in]      obstacle The number of the static obstacle to be inserted.
   * @param[in, out] rangeSq  The squared range around this agent.
   */
  public insertObstacleNeighbor(obstacle: Obstacle, rangeSq: number) {
    let nextObstacle: Obstacle = obstacle.next_;
    let distSq = 0;
    this.position_.sub(obstacle.point_, _v15);
    nextObstacle.point_.sub(obstacle.point_, _v16);
    this.position_.sub(nextObstacle.point_, _v17);

    let r = _v15.dot(_v16) / Vector2.absSq(_v16);

    if (r < 0) {
      distSq = Vector2.absSq(_v15);
    } else if (r > 1) {
      distSq = Vector2.absSq(_v17);
    } else {
      _v16.mulScalar(r).add(obstacle.point_);
      this.position_.sub(_v16, _v16);
      distSq = Vector2.absSq(_v16);
    }

    if (distSq < rangeSq) {
      let info = ObstacleNeighborInfo.pool.get();
      info.dist = distSq;
      info.obstacle = obstacle;
      this.obstacleNeighbors_.push(info);

      let i: number = this.obstacleNeighbors_.length - 1;

      while (i != 0 && distSq < this.obstacleNeighbors_.get(i - 1).dist) {
        // this.obstacleNeighbors_.set(i, this.obstacleNeighbors_.get(i - 1));
        this.obstacleNeighbors_.get(i).dist = this.obstacleNeighbors_.get(i - 1).dist;
        this.obstacleNeighbors_.get(i).obstacle = this.obstacleNeighbors_.get(i - 1).obstacle;
        --i;
      }
      info = this.obstacleNeighbors_.get(i);
      info.dist = distSq;
      info.obstacle = obstacle;
      // this.obstacleNeighbors_.set(i, [distSq, obstacle]);
    }
  }

  /**
   * @brief     Updates the two-dimensional position and two-dimensional
   *            velocity of this agent.
   * @param[in] timeStep The time step of the simulation.
   */
  public update(timeStep: number) {
    this.velocity_ = this.newVelocity_;
    this.position_.add(this.velocity_.mulScalar(timeStep, _v18));

    if (this.syncCallback) {
      this.syncCallback.call(this.syncTarget);
    }
  }

  public agentNeighbors_: RVOArray<AgentNeighborInfo>;
  public obstacleNeighbors_: RVOArray<ObstacleNeighborInfo>;

  public orcaLines_: RVOArray<Line>;

  public newVelocity_: Vector2;
  public position_: Vector2;
  public prefVelocity_: Vector2;
  public velocity_: Vector2;
  public maxNeighbors_: number;

  public maxSpeed_: number;
  public neighborDist_: number;
  public radius_: number;
  public timeHorizon_: number;
  public timeHorizonObst_: number;

  public syncCallback: Function;
  public syncTarget: any;
  public enable: boolean;
}
